1. Field of the Invention
The present invention relates to a falling prevention controlling device and a computer program to prevent even a monocycle from falling in a pitch direction.
2. Description of the Related Art
Conventionally, when, for example, two-wheeled vehicles and two-legged walking robots move, a main body swings in a roll direction, which has an axis extending in a substantially front-back direction, and therefore, it is necessary to control an operation to move or stop the vehicle or robot without falling by controlling the balance in the roll direction. To prevent falling in the roll direction, it is necessary to accurately detect the inclination in the roll direction. A method of detecting the inclination of the body to control the balance in the roll direction includes, for example, a method of detecting an angular velocity using an angular velocity sensor and estimating the inclination by integrating the detected value, and a method of detecting the inclination using a weight (deadweight).
However, with the method of integrating an angular velocity using an angular velocity sensor, when an angular velocity output detected by the angular velocity sensor includes a noise and an offset, the noise and the offset are amplified by integration. The amplified noise and offset accumulate, and therefore, there is a problem in that a shift from a target value increases and the estimation of an inclined angle and control of falling prevention cannot be effectively performed.
In contrast, with the method of detecting the inclination using a weight, the inclination cannot be reflected instantly due to the influence of the gravity, and therefore, the responsiveness of this method is poor and the inclination is detected in the vertical direction. Thus, there is a problem in that the inclination with respect to the balanced state cannot be detected. When, for example, the gravity center position is shifted in the roll direction, if, for example, a side wind is blowing, the balanced state is not necessarily maintained in the vertical direction. In order to prevent falling, it is necessary to restore the inclined state to the balanced state and detect the inclination with respect to the balanced state.
For example, WO 2007/063665 proposes a falling prevention controlling device which includes an angular velocity sensor which orients a detection axis toward a substantially front-back direction of the main body, a motor which orients a rotation axis in the substantially front-back direction of the main body, a rotation sensor which detects the rotation position or the rotation velocity of the motor, and an inertia rotor which is connected to the rotation axis of the motor, which estimates the inclined angle of the main body with respect to the balanced state, from the angular velocity output of the angular velocity sensor and the torque command provided to the motor, and which corrects the inclination of the main body using the estimated inclined angle.
According to WO 2007/063665, the angular velocity output does not need to integrated and, therefore, a noise and an offset are not accumulated. Moreover, the inclined angle with respect to the balanced state can be estimated, so that the inclined state can be corrected to the balanced state in the left-right direction using the estimated inclined angle. However, although, for example, two-wheeled vehicles can be prevented from falling as long as the balance in the left-right direction can be controlled, monocycles, for example, fall in the front-back direction if the balance in the front-back direction is not controlled even though the balance in the left-right direction is controlled.
According to “Monocycle Attitude Control”, Takashi KASAI, master thesis of Doctoral Course in Graduate School of System Information Technology, University of Tsukuba, January 2005, pages 1 to 37 (hereinafter “Kasai”), a gyro sensor and two acceleration sensors are used to estimate the inclination of a monocycle in the front-back direction. When acceleration sensors are spaced apart from the rotation axis, vibration is applied to the acceleration sensors, and therefore, a correct value is estimated by canceling the acceleration due to the vibration using the two acceleration sensors. The angle θa calculated from the two acceleration sensors and the angle θz of an integral value of the angular velocity obtained from the gyro sensor are output, θa and θz are filtered to cancel high frequency noise included therein, and the deviation e of the filtered values is input to provide as dθz the output multiplied by an observer gain, and use an estimated value θz hat obtained by correcting the angle θz by dθz for controlling.
According to Kasai, although the angle θz obtained by simply integrating the angular velocity acquired in the gyro sensor is shifted over time, the estimated value θz obtained by correcting the angle θz can maintain a constant value, so that it is possible to correct a shift of an offset of the angle calculated from the angular velocity acquired in the gyro sensor, by using the corrected estimated value θz hat.
However, with the monocycle of Kasai, although the inclination with respect to the gravity direction (vertical direction) is measured by acceleration sensors, the balanced state in the vertical direction is not necessarily maintained in the front-back direction similar to the above left-right direction. Therefore, there is a problem in that, when the balanced state is not maintained in the vertical direction, the inclined state cannot be corrected to the balanced state. Further, two acceleration sensors are required in addition to a gyro sensor to estimate the inclination, and therefore, there is a problem in that it is difficult to simplify and miniaturize a configuration of a falling prevention controlling device.